/*
 -------------------------------------------------------------------------------
 ServoGyro.h
 ServoGyro Project
 
This class is used to interface the Arduino with Servos that have been modified to
enable access to the servo's internal potentiometer's currently measured voltage.
 
 CONSTRUCTORS:
   	ServoGyro();
		Calls constructor with no parameters, i.e. minimum position is set to
		0 degrees, maximum position is set to 180 degrees
	ServoGyro(int angleRange);
		Calls ServoGyro constructor with one integer parameter specifying the 
		servo's maximally possible range in degrees. This assumes a minimum 
		position of 0 degrees
	ServoGyro(int minAngle, int maxAngle);   
		Calls constructor with two integer parameters specifying the servo's
		minimum and maximum position in degrees
 
 PROPERTIES:
 	int _minAngle;
		Specifies minimally reachable angle of the servo. Set in constructor.
	int _maxAngle;
		Specifies maximally reachable angle of the servo. Set in constructor.
	int _minVoltage;
		Specifies voltage corresponding to minimally reachable angle of the servo. Determined by calibration.
	int _maxVoltage;
		Specifies voltage corresponding to maximally reachable angle of the servo. Determined by calibration.
	int _minPulseWidth;
		Specifies pulse width corresponding to minimally reachable angle of the servo. Determined by calibration.
	int _maxPulseWidth;
		Specifies pulse width corresponding to maximally reachable angle of the servo. Determined by calibration.
	int _refAngle;
		Specifies angle the servo is currently trying to reach.
	bool _isCalibrated;
		Specifies whether the servo has been calibrated.
    
 METHODS:
	void calibrate(int inputPin, int outputPin, int calibrationTimeStep, int calibrationBreakThreshold);
		Determines minimum and maximum position of the servo by moving the servo head in small steps. 
		During each timestep determined by calibrationTimeStep, the pwm pulse width is decremented till
		the servo is in the minimum position and then moved incrementally to its maximum position. The 
		calibration results are stored in private member variables. The parameter calibrationBreakThreshold
		determines by how much the measured servo potentiometer voltage has to differ from the previous 
		measurement to be recognized as the servo's minimum/maximum position.
		The parameter inputPin specifies at which pin the servo potentiometer is connected to and outputPin
		specifies the digital pwm output pin used to control the servo. 
		
 	void calibrate(int inputPin, int outputPin);
 		Calls the calibrate method with default values for calibrationTimeStep and calibrationBreakThreshold,
 		which are 250ms and 50.

	void writeAngle(int angle);
		Moves the servo to the angle specified. If the angle is out of range of the servo, it moves the 
		servo to its minimum/maximum position. Can just be called after the servo has been calibrated, 
		otherwise it will fail.
		
	int readAngle(int inputPin);
		Returns the actual angle of the servo.
		
	int getAngle();
		Returns the _refAngle that the servo is trying to move to.
		
	void goToMin(int inputPin);
		Moves the servo to its minimum position if it has been calibrated. Otherwise it will return an error.

	void goToMax(int inputPin);
		Moves the servo to its maximum position if it has been calibrated. Otherwise it will return an error.
		
	void goToMiddle(int inputPin);
		Moves the servo to its middle position if it has been calibrated. Otherwise it will return an error.
 
 NOTES:
 
 EXAMPLES:
 
 	#include <Servo.h>
	#include <ServoGyro.h>

	ServoGyro servo1;

	void setup() {
  		pinMode(6,OUTPUT);
  		servo1.attach(6);
  		analogReference(INTERNAL);	
		servo1.calibrate(5,6, 250, 50);
	}
 
 Initially created by Daniel Pickem on 1/20/12.
 
 Version 1.0
 -------------------------------------------------------------------------------
 */

#ifndef _ServoGyro_h_
#define _ServoGyro_h_

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include <Arduino.h>
#include <Servo.h>

class ServoGyro : public Servo {
	//--------------------------------------------------------------------------
	// Friends
	//--------------------------------------------------------------------------
	
	
	//--------------------------------------------------------------------------
	// Friend Overloaded Operators
	//--------------------------------------------------------------------------
	
	
public:

	//--------------------------------------------------------------------------
	// Constants, Enums, and Types
	//--------------------------------------------------------------------------
	
	
	//--------------------------------------------------------------------------
	// Lifecycle
	//--------------------------------------------------------------------------
	
	// Constructors
	ServoGyro();
	ServoGyro(int angleRange);
	ServoGyro(int minAngle, int maxAngle);
	
	// Destructor
	virtual ~ServoGyro();
	
	// Copy constructor
	ServoGyro(const ServoGyro& srcObj);
	
	// Assignment operator
	const ServoGyro& operator=(const ServoGyro& rhsObj);
	
	
	//--------------------------------------------------------------------------
	// Overloaded Operators
	//--------------------------------------------------------------------------
	
	
	//--------------------------------------------------------------------------
	// Public Member Functions
	//--------------------------------------------------------------------------
	void calibrate(int inputPin, int outputPin);
	void calibrate(int inputPin, int outputPin, int calibrationTimeStep, int calibrationBreakThreshold);
  void calibrateWithData(int minVoltage, int maxVoltage, int minPulseWidth, int maxPulseWidth);
	void writeAngle(int angle);
	int readAngle(int inputPin);	// returns the actual angle the servo is at
	int getAngle();			// returns the _refAngle that the servo is trying to move to
	void goToMin(int inputPin);
	void goToMax(int inputPin);
	void goToMiddle(int inputPin);
	
	int getMinAngle();
	int getMaxAngle();
	int getMinVoltage();
	int getMaxVoltage();
	int getMinPulseWidth();
	int getMaxPulseWidth();
    
	//--------------------------------------------------------------------------
	// Public Member Variables
	//--------------------------------------------------------------------------
	
protected:
	//--------------------------------------------------------------------------
	// Protected Member Functions
	//--------------------------------------------------------------------------
	
	
	//--------------------------------------------------------------------------
	// Protected Member Variables
	//--------------------------------------------------------------------------
	
	
private:
	//--------------------------------------------------------------------------
	// Lifecycle
	//--------------------------------------------------------------------------
	void copyHelper(const ServoGyro& srcObj);
	
	//--------------------------------------------------------------------------
	// Private Member Functions
	//--------------------------------------------------------------------------
	
	//--------------------------------------------------------------------------
	// Private Member Variables
	//--------------------------------------------------------------------------
	// Calibration variables
	int _minAngle;
	int _maxAngle;
	int _minVoltage;
	int _maxVoltage;
	int _minPulseWidth;
	int _maxPulseWidth;
	int _refAngle;
	bool _isCalibrated;
};

#endif
